#include <string.h>
#include <stdlib.h>
#include <math.h>

#include "rs274ngc.hh"
#include "rs274ngc_return.hh"
#include "interp_queue.hh"
#include "interp_internal.hh"
#include "rs274ngc_interp.hh"

static int debug_qc = 0;

// lathe tools have strange origin points that are not at
// the center of the radius.  This means that the point that
// radius compensation controls (center of radius) is not at
// the tool's origin.  These functions do the necessary
// translation.  Notice tool orientations 0 (mill) and 9, and
// those with radius 0 (a point) do not need any translation.

static double latheorigin_x(setup_pointer settings, double x)
{
    int o = settings->cutter_comp_orientation;
    double r = settings->cutter_comp_radius;
    if (settings->plane != CANON_PLANE_XZ)
        return x;

    if (o == 2 || o == 6 || o == 1)
        x -= r;
    if (o == 3 || o == 8 || o == 4)
        x += r;
    return x;
}

static double latheorigin_z(setup_pointer settings, double z)
{
    int o = settings->cutter_comp_orientation;
    double r = settings->cutter_comp_radius;
    if (settings->plane != CANON_PLANE_XZ)
        return z;

    if (o == 2 || o == 7 || o == 3)
        z -= r;
    if (o == 1 || o == 5 || o == 4)
        z += r;
    return z;
}

static double endpoint[2];
static int endpoint_valid = 0;

std::vector<queued_canon>& qc(void)
{
    static std::vector<queued_canon> c;
    if (0)
        printf("len %d\n", (int)c.size());
    return c;
}

void qc_reset(void)
{
    if (debug_qc)
        printf("qc cleared\n");
    qc().clear();
    endpoint_valid = 0;
}

void enqueue_SET_FEED_RATE(double feed)
{
    if (qc().empty())
    {
        if (debug_qc)
            printf("immediate set feed rate %f\n", feed);
        SET_FEED_RATE(feed);
        return;
    }
    queued_canon q;
    q.type = QSET_FEED_RATE;
    q.data.set_feed_rate.feed = feed;
    if (debug_qc)
        printf("enqueue set feed rate %f\n", feed);
    qc().push_back(q);
}

void enqueue_DWELL(int line_num, double time, short sub_thread)
{
    if (qc().empty())
    {
        if (debug_qc)
            printf("immediate dwell %f\n", time);
        DWELL(time, line_num, sub_thread);
        return;
    }
    queued_canon q;
    q.thread_id = sub_thread;
    q.line_num = line_num;
    q.type = QDWELL;
    q.data.dwell.time = time;
    if (debug_qc)
        printf("enqueue dwell %f\n", time);
    qc().push_back(q);
}

void enqueue_ENABLEADMIT(int enable)
{
    if (qc().empty())
    {
        if (debug_qc)
            printf("immediate enableadmit %d\n", enable);
        ENABLE_ADMIT(enable);
        return;
    }
    queued_canon q;
    q.type = QENABLEADMIT;
    q.data.enable_admit.enable = enable;
    if (debug_qc)
        printf("enqueue enableadmit %d\n", enable);
    qc().push_back(q);
}

void enqueue_SET_FEED_MODE(int mode)
{
    if (qc().empty())
    {
        if (debug_qc)
            printf("immediate set feed mode %d\n", mode);
        SET_FEED_MODE(mode);
        return;
    }
    queued_canon q;
    q.type = QSET_FEED_MODE;
    q.data.set_feed_mode.mode = mode;
    if (debug_qc)
        printf("enqueue set feed mode %d\n", mode);
    qc().push_back(q);
}

void enqueue_SET_TOOL(int line_num, setup_pointer settings, double* toolOffset)
{
    if (qc().empty())
    {
        SETTOOL(line_num, toolOffset, settings->joints_current, &settings->user_offset);
        return;
    }
    queued_canon q;
    q.line_num = line_num;
    q.type = QSETTOOL;
    q.data.settool.x_off = toolOffset[0];
    q.data.settool.y_off = toolOffset[1];
    q.data.settool.z_off = toolOffset[2];
    q.data.settool.a_off = toolOffset[3];
    q.data.settool.b_off = toolOffset[4];
    q.data.settool.c_off = toolOffset[5];
    qc().push_back(q);
}

void enqueue_SET_USER_FRAME(int line_num, setup_pointer settings, double* userFrame)
{
    if (qc().empty())
    {
        SETUSERFRAME(line_num, userFrame, settings->joints_current, &settings->user_offset);
        return;
    }
    queued_canon q;
    q.line_num = line_num;
    q.type = QSETUSERFRAME;
    q.data.setuserframe.x_off = userFrame[0];
    q.data.setuserframe.y_off = userFrame[1];
    q.data.setuserframe.z_off = userFrame[2];
    q.data.setuserframe.a_off = userFrame[3];
    q.data.setuserframe.b_off = userFrame[4];
    q.data.setuserframe.c_off = userFrame[5];
    qc().push_back(q);
}

void enqueue_SET_PAYLOAD(int line_num, setup_pointer settings, Payload payload)
{
    if (qc().empty())
    {
        SET_PAYLOAD(line_num, payload);
        return;
    }

    queued_canon q;
    q.line_num = line_num;
    q.type = QSETPAYLOAD;
    q.data.setpayload.payload = payload;
    qc().push_back(q);
}

void enqueue_SET_ADMITCTRLCONFIG(int line_num, setup_pointer settings, int axis, FtConfig ftConfig, int now)
{
    if (qc().empty())
    {
        SET_ADMITCTRLCONFIG(line_num, axis, ftConfig, now);
        return;
    }

    queued_canon q;
    q.line_num = line_num;
    q.type = QSETADMITCTRLCONFIG;
    q.data.setftconfig.axis = axis;
    q.data.setftconfig.ftConfig = ftConfig;
    q.data.setftconfig.now = now;
    qc().push_back(q);
}

void enqueue_SET_ENDFORCECOND(int line_num, setup_pointer settings, int axis, EndForceCond endForceCond)
{
    if (qc().empty())
    {
        SET_ENDFORCECOND(line_num, axis, endForceCond);
        return;
    }

    queued_canon q;
    q.line_num = line_num;
    q.type = QSETENDFORCECOND;
    q.data.setendforcecond.axis = axis;
    q.data.setendforcecond.endForceCond = endForceCond;
    qc().push_back(q);
}

void enqueue_SET_COMPLIANT_TYPE(int line_num, setup_pointer settings, int* typeEna)
{
    if (qc().empty())
    {
        SET_COMPLIANTTYPE(line_num, typeEna);
        return;
    }

    queued_canon q;
    q.line_num = line_num;
    q.type = QSETCOMPLIANTTYPE;
    q.data.setcomplianttype.compliantType = typeEna[0];
    q.data.setcomplianttype.compliantEnable = typeEna[1];

    qc().push_back(q);
}

void enqueue_SET_FORCE_CTRL_FRAME(int line_num, setup_pointer settings, int ftFrame)
{
    if (qc().empty())
    {
        SET_FORCRCTRLFRAME(line_num, ftFrame);
        return;
    }

    queued_canon q;
    q.line_num = line_num;
    q.type = QSETFORCECTRLFRAME;
    q.data.setforcecontrolframe.ftFrame = ftFrame;
    qc().push_back(q);
}

void enqueue_DISABLE_FORCE_CONTROL(int line_num, setup_pointer settings, int disableForceCtrl)
{
    if (qc().empty())
    {
        DISABLE_FORCECONTROL(line_num, disableForceCtrl);
        return;
    }

    queued_canon q;
    q.line_num = line_num;
    q.type = QDISABLEFORCECONTROL;
    q.data.disableforcecontrol.disableForceCtrl = disableForceCtrl;
    qc().push_back(q);
}

void enqueue_SET_VEL_COMPLIANT_LEVEL(int line_num, setup_pointer settings, double* velCompliantCtrl)
{
    if (qc().empty())
    {
        SET_VELCOMPLIANTLEVEL(line_num, velCompliantCtrl);
        return;
    }

    queued_canon q;
    q.line_num = line_num;
    q.type = QSETVELCOMPLIANTLEVEL;
    q.data.setvelcompliantlevel.opt = velCompliantCtrl[0];
    q.data.setvelcompliantlevel.rate1 = velCompliantCtrl[1];
    q.data.setvelcompliantlevel.rate2 = velCompliantCtrl[2];
    q.data.setvelcompliantlevel.rate3 = velCompliantCtrl[3];
    q.data.setvelcompliantlevel.rate4 = velCompliantCtrl[4];
    qc().push_back(q);
}

void enqueue_SET_COMPLIANT_CONDITION(int line_num, setup_pointer settings, double* compliantCondition)
{
    if (qc().empty())
    {
        SET_COMPLIANTCON(line_num, compliantCondition);
        return;
    }

    queued_canon q;
    q.line_num = line_num;
    q.type = QSETCOMPLIANTCON;

    q.data.setcompliantcon.fx = compliantCondition[0];
    q.data.setcompliantcon.fy = compliantCondition[1];
    q.data.setcompliantcon.fz = compliantCondition[2];
    q.data.setcompliantcon.tx = compliantCondition[3];
    q.data.setcompliantcon.ty = compliantCondition[4];
    q.data.setcompliantcon.tz = compliantCondition[5];

    qc().push_back(q);
}

void enqueue_SET_CLSN_LEVEL(int line_num, setup_pointer settings, int clsnLevel)
{
    if (qc().empty())
    {
        SET_CLSN_LEVEL(line_num, clsnLevel);
        return;
    }
    queued_canon q;
    q.line_num = line_num;
    q.type = QSETCLSNLEVEL;
    q.data.setclsnlevel.clsn_level = clsnLevel;
    qc().push_back(q);
}

void enqueue_COMMENT(const char* c)
{
    if (qc().empty())
    {
        if (debug_qc)
            printf("immediate comment \"%s\"\n", c);
        COMMENT(c);
        return;
    }
    queued_canon q;
    q.type = QCOMMENT;
    q.data.comment.comment = strdup(c);
    if (debug_qc)
        printf("enqueue comment \"%s\"\n", c);
    qc().push_back(q);
}

int enqueue_STRAIGHT_FEED(setup_pointer settings,
                          int l,
                          double dx,
                          double dy,
                          double dz,
                          double x,
                          double y,
                          double z,
                          double a,
                          double b,
                          double c,
                          double u,
                          double v,
                          double w)
{
    queued_canon q;
    q.type = QSTRAIGHT_FEED;
    q.line_num = l;
    switch (settings->plane)
    {
    case CANON_PLANE_XY:
        q.data.straight_feed.dx = dx;
        q.data.straight_feed.dy = dy;
        q.data.straight_feed.dz = dz;
        q.data.straight_feed.x = x;
        q.data.straight_feed.y = y;
        q.data.straight_feed.z = z;
        break;
    case CANON_PLANE_XZ:
        q.data.straight_feed.dz = dx;
        q.data.straight_feed.dx = dy;
        q.data.straight_feed.dy = dz;
        q.data.straight_feed.z = x;
        q.data.straight_feed.x = y;
        q.data.straight_feed.y = z;
        break;
    default:;
    }
    q.data.straight_feed.a = a;
    q.data.straight_feed.b = b;
    q.data.straight_feed.c = c;
    q.data.straight_feed.u = u;
    q.data.straight_feed.v = v;
    q.data.straight_feed.w = w;
    qc().push_back(q);
    if (debug_qc)
        printf("enqueue straight feed lineno %d to %f %f %f direction %f %f %f\n", l, x, y, z, dx, dy, dz);
    return 0;
}

int enqueue_STRAIGHT_TRAVERSE(setup_pointer settings,
                              int l,
                              double dx,
                              double dy,
                              double dz,
                              double x,
                              double y,
                              double z,
                              double a,
                              double b,
                              double c,
                              double u,
                              double v,
                              double w)
{
    queued_canon q;
    q.type = QSTRAIGHT_TRAVERSE;
    q.line_num = l;
    switch (settings->plane)
    {
    case CANON_PLANE_XY:
        q.data.straight_traverse.dx = dx;
        q.data.straight_traverse.dy = dy;
        q.data.straight_traverse.dz = dz;
        q.data.straight_traverse.x = x;
        q.data.straight_traverse.y = y;
        q.data.straight_traverse.z = z;
        break;
    case CANON_PLANE_XZ:
        q.data.straight_traverse.dz = dx;
        q.data.straight_traverse.dx = dy;
        q.data.straight_traverse.dy = dz;
        q.data.straight_traverse.z = x;
        q.data.straight_traverse.x = y;
        q.data.straight_traverse.y = z;
        break;
    default:;
    }
    q.data.straight_traverse.a = a;
    q.data.straight_traverse.b = b;
    q.data.straight_traverse.c = c;
    q.data.straight_traverse.u = u;
    q.data.straight_traverse.v = v;
    q.data.straight_traverse.w = w;
    if (debug_qc)
        printf("enqueue straight traverse lineno %d to %f %f %f direction %f %f %f\n", l, x, y, z, dx, dy, dz);
    qc().push_back(q);
    return 0;
}

void enqueue_ARC_FEED(setup_pointer settings,
                      int l,
                      double original_turns,
                      double end1,
                      double end2,
                      double center1,
                      double center2,
                      int turn,
                      double end3,
                      double a,
                      double b,
                      double c,
                      double u,
                      double v,
                      double w)
{
    queued_canon q;

    q.type = QARC_FEED;
    q.line_num = l;
    q.data.arc_feed.original_turns = original_turns;
    q.data.arc_feed.end1 = end1;
    q.data.arc_feed.end2 = end2;
    q.data.arc_feed.center1 = center1;
    q.data.arc_feed.center2 = center2;
    q.data.arc_feed.turn = turn;
    q.data.arc_feed.end3 = end3;
    q.data.arc_feed.a = a;
    q.data.arc_feed.b = b;
    q.data.arc_feed.c = c;
    q.data.arc_feed.u = u;
    q.data.arc_feed.v = v;
    q.data.arc_feed.w = w;

    if (debug_qc)
        printf("enqueue arc lineno %d to %f %f center %f %f turn %d sweeping %f\n", l, end1, end2, center1, center2, turn, original_turns);
    qc().push_back(q);
}

void enqueue_M_USER_COMMAND(int index, double p_number, double q_number)
{
    if (qc().empty())
    {
        if (debug_qc)
            printf("immediate M_USER_COMMAND index=%d p=%f q=%f\n", index, p_number, q_number);
        (*(USER_DEFINED_FUNCTION[index - 100]))(index - 100, p_number, q_number);
        return;
    }
    queued_canon q;
    q.type = QM_USER_COMMAND;
    q.data.mcommand.index = index;
    q.data.mcommand.p_number = p_number;
    q.data.mcommand.q_number = q_number;
    if (debug_qc)
        printf("enqueue M_USER_COMMAND index=%d p=%f q=%f\n", index, p_number, q_number);
    qc().push_back(q);
}

void qc_scale(double scale)
{
    if (qc().empty())
    {
        if (debug_qc)
            printf("not scaling because qc is empty\n");
        return;
    }

    if (debug_qc)
        printf("scaling qc by %f\n", scale);

    for (unsigned int i = 0; i < qc().size(); i++)
    {
        queued_canon& q = qc()[i];
        endpoint[0] *= scale;
        endpoint[1] *= scale;
        switch (q.type)
        {
        case QARC_FEED:
            q.data.arc_feed.end1 *= scale;
            q.data.arc_feed.end2 *= scale;
            q.data.arc_feed.end3 *= scale;
            q.data.arc_feed.center1 *= scale;
            q.data.arc_feed.center2 *= scale;
            q.data.arc_feed.u *= scale;
            q.data.arc_feed.v *= scale;
            q.data.arc_feed.w *= scale;
            break;
        case QSTRAIGHT_FEED:
            q.data.straight_feed.x *= scale;
            q.data.straight_feed.y *= scale;
            q.data.straight_feed.z *= scale;
            q.data.straight_feed.u *= scale;
            q.data.straight_feed.v *= scale;
            q.data.straight_feed.w *= scale;
            break;
        case QSTRAIGHT_TRAVERSE:
            q.data.straight_traverse.x *= scale;
            q.data.straight_traverse.y *= scale;
            q.data.straight_traverse.z *= scale;
            q.data.straight_traverse.u *= scale;
            q.data.straight_traverse.v *= scale;
            q.data.straight_traverse.w *= scale;
            break;
        default:;
        }
    }
}

void dequeue_canons(setup_pointer settings)
{
    if (debug_qc)
        printf("dequeueing: endpoint is now invalid\n");
    endpoint_valid = 0;

    if (qc().empty())
        return;

    for (unsigned int i = 0; i < qc().size(); i++)
    {
        queued_canon& q = qc()[i];

        switch (q.type)
        {
        case QARC_FEED:
            if (debug_qc)
                printf("issuing arc feed lineno %d\n", q.line_num);
            ARC_FEED(q.line_num,
                     latheorigin_z(settings, q.data.arc_feed.end1),
                     latheorigin_x(settings, q.data.arc_feed.end2),
                     latheorigin_z(settings, q.data.arc_feed.center1),
                     latheorigin_x(settings, q.data.arc_feed.center2),
                     q.data.arc_feed.turn,
                     q.data.arc_feed.end3,
                     q.data.arc_feed.a,
                     q.data.arc_feed.b,
                     q.data.arc_feed.c,
                     q.data.arc_feed.u,
                     q.data.arc_feed.v,
                     q.data.arc_feed.w);
            break;
        case QSTRAIGHT_FEED:
            if (debug_qc)
                printf("issuing straight feed lineno %d\n", q.line_num);
            STRAIGHT_FEED(q.line_num,
                          latheorigin_x(settings, q.data.straight_feed.x),
                          q.data.straight_feed.y,
                          latheorigin_z(settings, q.data.straight_feed.z),
                          q.data.straight_feed.a,
                          q.data.straight_feed.b,
                          q.data.straight_feed.c,
                          q.data.straight_feed.u,
                          q.data.straight_feed.v,
                          q.data.straight_feed.w);
            break;
        case QSTRAIGHT_TRAVERSE:
            if (debug_qc)
                printf("issuing straight traverse lineno %d\n", q.line_num);
            STRAIGHT_TRAVERSE(q.line_num,
                              latheorigin_x(settings, q.data.straight_traverse.x),
                              q.data.straight_traverse.y,
                              latheorigin_z(settings, q.data.straight_traverse.z),
                              q.data.straight_traverse.a,
                              q.data.straight_traverse.b,
                              q.data.straight_traverse.c,
                              q.data.straight_traverse.u,
                              q.data.straight_traverse.v,
                              q.data.straight_traverse.w);
            break;
        case QSET_FEED_RATE:
            if (debug_qc)
                printf("issuing set feed rate\n");
            SET_FEED_RATE(q.data.set_feed_rate.feed);
            break;
        case QDWELL:
            if (debug_qc)
                printf("issuing dwell\n");
            DWELL(q.line_num, q.data.dwell.time, q.thread_id);
            break;
        case QENABLEADMIT:
            if (debug_qc)
                printf("issuing enable admit\n");
            ENABLE_ADMIT(q.data.enable_admit.enable);
            break;
        case QSETTOOL:
        {
            if (debug_qc)
                printf("issuing settool\n");
            double toolOffset[6] = {
                q.data.settool.x_off, q.data.settool.y_off, q.data.settool.z_off, q.data.settool.a_off, q.data.settool.b_off, q.data.settool.c_off};
            SETTOOL(q.line_num, toolOffset, settings->joints_current, &settings->user_offset);
            break;
        }
        case QSETUSERFRAME:
        {
            if (debug_qc)
                printf("issuing setuserfram\n");
            double userFrame[6] = {q.data.setuserframe.x_off,
                                   q.data.setuserframe.y_off,
                                   q.data.setuserframe.z_off,
                                   q.data.setuserframe.a_off,
                                   q.data.setuserframe.b_off,
                                   q.data.setuserframe.c_off};
            SETUSERFRAME(q.line_num, userFrame, settings->joints_current, &settings->user_offset);
            break;
        }
        case QSETCOMPLIANTCON:
        {
            if (debug_qc)
                printf("issuing compliantCondition\n");
            double compliantCondition[6] = {q.data.setcompliantcon.fx,
                                            q.data.setcompliantcon.fy,
                                            q.data.setcompliantcon.fz,
                                            q.data.setcompliantcon.tx,
                                            q.data.setcompliantcon.ty,
                                            q.data.setcompliantcon.tz};
            SET_COMPLIANTCON(q.line_num, compliantCondition);
            break;
        }
        case QSETVELCOMPLIANTLEVEL:
        {
            if (debug_qc)
                printf("issuing velCompliantCtrl\n");
            double velCompliantCtrl[5] = {q.data.setvelcompliantlevel.opt,
                                          q.data.setvelcompliantlevel.rate1,
                                          q.data.setvelcompliantlevel.rate2,
                                          q.data.setvelcompliantlevel.rate3,
                                          q.data.setvelcompliantlevel.rate4};
            SET_VELCOMPLIANTLEVEL(q.line_num, velCompliantCtrl);
            break;
        }
        case QSETADMITCTRLCONFIG:
        {
            if (debug_qc)
                printf("issuing setftconfig\n");
            SET_ADMITCTRLCONFIG(q.line_num, q.data.setftconfig.axis, q.data.setftconfig.ftConfig, q.data.setftconfig.now);
            break;
        }

        case QSETENDFORCECOND:
        {
            if (debug_qc)
                printf("issuing endForceCond\n");
            SET_ENDFORCECOND(q.line_num, q.data.setendforcecond.axis, q.data.setendforcecond.endForceCond);
            break;
        }

        case QSETCOMPLIANTTYPE:
        {
            if (debug_qc)
                printf("issuing compliantCondition\n");
            int typeEna[2] = {q.data.setcomplianttype.compliantType, q.data.setcomplianttype.compliantEnable};
            SET_COMPLIANTTYPE(q.line_num, typeEna);
            break;
        }

        case QSETFORCECTRLFRAME:
        {
            if (debug_qc)
                printf("issuing compliantCondition\n");
            int ftFrame = q.data.setforcecontrolframe.ftFrame;
            SET_FORCRCTRLFRAME(q.line_num, ftFrame);
            break;
        }

        case QDISABLEFORCECONTROL:
        {
            if (debug_qc)
                printf("issuing QDISABLEFORCECONTROL\n");
            int disableForceCtrl = q.data.disableforcecontrol.disableForceCtrl;
            DISABLE_FORCECONTROL(q.line_num, disableForceCtrl);
            break;
        }

        case QSETPAYLOAD:
        {
            if (debug_qc)
                printf("issuing setpayload\n");
            SET_PAYLOAD(q.line_num, q.data.setpayload.payload);
            break;
        }
        case QSETCLSNLEVEL:
        {
            if (debug_qc)
                printf("issuing setclsnlevel\n");
            SET_CLSN_LEVEL(q.line_num, q.data.setclsnlevel.clsn_level);
            break;
        }
        case QSET_FEED_MODE:
            if (debug_qc)
                printf("issuing set feed mode\n");
            SET_FEED_MODE(q.data.set_feed_mode.mode);
            break;
        case QCOMMENT:
            if (debug_qc)
                printf("issuing comment\n");
            COMMENT(q.data.comment.comment);
            free(q.data.comment.comment);
            break;
        case QM_USER_COMMAND:
            if (debug_qc)
                printf("issuing mcommand\n");
            {
                int index = q.data.mcommand.index;
                (*(USER_DEFINED_FUNCTION[index - 100]))(index - 100, q.data.mcommand.p_number, q.data.mcommand.q_number);
            }
            break;
        }
    }
    qc().clear();
}

int Interp::move_endpoint_and_flush(setup_pointer settings, double x, double y)
{
    double x1;
    double y1;
    double x2;
    double y2;
    double dot;

    if (qc().empty())
        return 0;

    for (unsigned int i = 0; i < qc().size(); i++)
    {
        // there may be several moves in the queue, and we need to
        // change all of them.  consider moving into a concave corner,
        // then up and back down, then continuing on.  there will be
        // three moves to change.

        queued_canon& q = qc()[i];

        switch (q.type)
        {
        case QARC_FEED:
            double r1, r2, l1, l2;
            r1 = hypot(q.data.arc_feed.end1 - q.data.arc_feed.center1, q.data.arc_feed.end2 - q.data.arc_feed.center2);
            l1 = q.data.arc_feed.original_turns;
            q.data.arc_feed.end1 = x;
            q.data.arc_feed.end2 = y;
            r2 = hypot(x - q.data.arc_feed.center1, y - q.data.arc_feed.center2);
            l2 = find_turn(endpoint[0], endpoint[1], q.data.arc_feed.center1, q.data.arc_feed.center2, q.data.arc_feed.turn, x, y);
            if (debug_qc)
                printf("moving endpoint of arc lineno %d old sweep %f new sweep %f\n", q.line_num, l1, l2);

            if (fabs(r1 - r2) > .01)
                ERS(INTERP_ERR_INTERNAL, _("BUG: cutter compensation has generated an invalid arc with mismatched radii r1 %f r2 %f"), r1, r2);
            if (l1 && endpoint_valid && fabs(l2) > fabs(l1) + (settings->length_units == CANON_UNITS_MM ? .0254 : .001))
            {
                ERS(INTERP_ERR_INTERNAL, _("Arc move in concave corner cannot be reached by the tool without gouging"));
            }
            q.data.arc_feed.end1 = x;
            q.data.arc_feed.end2 = y;
            break;
        case QSTRAIGHT_TRAVERSE:
            switch (settings->plane)
            {
            case CANON_PLANE_XY:
                x1 = q.data.straight_traverse.dx;  // direction of original motion
                y1 = q.data.straight_traverse.dy;
                x2 = x - endpoint[0];  // new direction after clipping
                y2 = y - endpoint[1];
                break;
            case CANON_PLANE_XZ:
                x1 = q.data.straight_traverse.dz;  // direction of original motion
                y1 = q.data.straight_traverse.dx;
                x2 = x - endpoint[0];  // new direction after clipping
                y2 = y - endpoint[1];
                break;
            default:
                ERS(INTERP_ERR_INTERNAL, _("BUG: Unsupported plane in cutter compensation"));
            }

            dot = x1 * x2 + y1 * y2;  // not normalized; we only care about the angle
            if (debug_qc)
                printf("moving endpoint of traverse old dir %f new dir %f dot %f endpoint_valid %d\n", atan2(y1, x1), atan2(y2, x2), dot, endpoint_valid);

            if (endpoint_valid && dot < 0)
            {
                // oops, the move is the wrong way.  this means the
                // path has crossed because we backed up further
                // than the line is long.  this will gouge.
                ERS(INTERP_ERR_INTERNAL, _("Straight traverse in concave corner cannot be reached by the tool without gouging"));
            }
            switch (settings->plane)
            {
            case CANON_PLANE_XY:
                q.data.straight_traverse.x = x;
                q.data.straight_traverse.y = y;
                break;
            case CANON_PLANE_XZ:
                q.data.straight_traverse.z = x;
                q.data.straight_traverse.x = y;
                break;
            }
            break;
        case QSTRAIGHT_FEED:
            switch (settings->plane)
            {
            case CANON_PLANE_XY:
                x1 = q.data.straight_feed.dx;  // direction of original motion
                y1 = q.data.straight_feed.dy;
                x2 = x - endpoint[0];  // new direction after clipping
                y2 = y - endpoint[1];
                break;
            case CANON_PLANE_XZ:
                x1 = q.data.straight_feed.dz;  // direction of original motion
                y1 = q.data.straight_feed.dx;
                x2 = x - endpoint[0];  // new direction after clipping
                y2 = y - endpoint[1];
                break;
            default:
                ERS(INTERP_ERR_INTERNAL, _("BUG: Unsupported plane [%d] in cutter compensation"), settings->plane);
            }

            dot = x1 * x2 + y1 * y2;
            if (debug_qc)
                printf("moving endpoint of feed old dir %f new dir %f dot %f endpoint_valid %d\n", atan2(y1, x1), atan2(y2, x2), dot, endpoint_valid);

            if (endpoint_valid && dot < 0)
            {
                // oops, the move is the wrong way.  this means the
                // path has crossed because we backed up further
                // than the line is long.  this will gouge.
                ERS(INTERP_ERR_INTERNAL, _("Straight feed in concave corner cannot be reached by the tool without gouging"));
            }
            switch (settings->plane)
            {
            case CANON_PLANE_XY:
                q.data.straight_feed.x = x;
                q.data.straight_feed.y = y;
                break;
            case CANON_PLANE_XZ:
                q.data.straight_feed.z = x;
                q.data.straight_feed.x = y;
                break;
            }
            break;
        default:
            // other things are not moves - we don't have to mess with them.
            ;
        }
    }
    dequeue_canons(settings);
    set_endpoint(x, y);
    return 0;
}

void set_endpoint(double x, double y)
{
    if (debug_qc)
        printf("setting endpoint %f %f\n", x, y);
    endpoint[0] = x;
    endpoint[1] = y;
    endpoint_valid = 1;
}
